#coding=utf-8
# !/usr/bin/python
# -*- coding: utf-8 -*-
from ctypes import *

class c_highspeed:
    def __init__(self):
        self.a_pe = []
        self.a_pec = []
        self.a_mspeed = []
        self.a_mpos = []
        self.a_rpos = []
        self.a_new = []

        self.ga_x = [] # 起始点x
        self.ga_pe = [] # 起始点y
        self.ga_pec = [] # 起始点y
        self.ga_mspeed = [] # 起始点y
        self.ga_new = [] # 
        self.ga_mpos = [] # 起始点y
        self.ga_rpos = [] # 起始点y
        self.dt_xmax = 16000
        self.dt_framelen = 15
        self.frame_num = 0
        self.x_idx = 0
        
    def find_head(self, a):
        i = 0
        data_len = self.dt_framelen - 1
        while i < self.dt_framelen:    
            if a[i] == 0xA0 and a[i + data_len] == 0x0A:
                return i
            i+=1
        return 0

    def handle_data(self, a_pe, a_pec, a_mspeed, a_mpos, a_rpos, a_new):
  
        ret = 0
        
        if 0 != len(a_pe):
            for i in range(0, len(a_pe)):
                self.ga_x.append( (i + self.x_idx)/1000 )
            self.x_idx+=  len(a_pe) 
            self.ga_pe+= a_pe
            self.ga_pec+= a_pec
            self.ga_mspeed+= a_mspeed
            self.ga_mpos+= a_mpos
            self.ga_rpos+= a_rpos
            self.ga_new+= a_new
            del_len = len(self.ga_pe) - self.dt_xmax
            if del_len > 0 :
                self.ga_x = self.ga_x[del_len:]
                self.ga_pe = self.ga_pe[del_len:]
                self.ga_pec = self.ga_pec[del_len:]
                self.ga_mspeed = self.ga_mspeed[del_len:]
                self.ga_mpos = self.ga_mpos[del_len:]
                self.ga_rpos = self.ga_rpos[del_len:]
                self.ga_new = self.ga_new[del_len:]
            if del_len != 0 :    
                ret = 1
        return ret
 

    def parse_frame(self, h):
        pe = ( (h[2] << 8) + h[3] )
        pec = ( (h[4] << 8) + h[5])
        speed = ( (h[6] << 8) + h[7])
        new = ( (h[8] << 8) + h[9])
        disp = ( (h[10] << 8) + h[11]) 
        rdisp = ( (h[12] << 8) + h[13]) 
        
        self.a_pe.append(c_int16(pe).value)
        self.a_pec.append(c_int16(pec).value)
        self.a_mspeed.append(c_int16(speed).value)
        self.a_mpos.append(c_int16(disp).value)
        self.a_rpos.append(c_int16(rdisp).value)
        self.a_new.append(c_int16(new).value)


    def parse_block(self, h):
        data_len = self.dt_framelen - 1
        i = 0
        err = 0
        self.a_pe.clear()
        self.a_pec.clear()
        self.a_mspeed.clear()
        self.a_mpos.clear()
        self.a_rpos.clear()
        self.a_new.clear()
        while i < len(h) - data_len :
            if h[i] == 0xA0 and h[i + data_len] == 0x0A:      
                self.frame_num = self.frame_num + 1
                self.parse_frame(h[i:i + data_len])
                i+= self.dt_framelen
            else:
                i+=1
                err+=1   
        if 0 != err:
            print("frame", self.frame_num, "err",  err,  flush = True) 
 
        return self.handle_data(self.a_pe, self.a_pec, self.a_mspeed, self.a_mpos, self.a_rpos, self.a_new)


